4.1 LQR

0. 前言

1.1 从"能控"到"最优"

控制理论的发展可以粗略地划分为两个阶段：

经典控制（PID 为代表）：不依赖数学模型，靠误差反馈来调节。简单好用，但在多变量耦合系统上捉襟见肘。
现代控制（状态空间方法）：基于系统的数学模型，直接操纵系统的内部状态。

在现代控制中，设计一个控制器通常意味着：给定系统 $\dot{x} = A x + B u$ ，找一个反馈增益 $K$ ，使得 $u = - K x$ 能让系统稳定。

四旋翼飞行器属于高阶多变量非线性系统，传统PID控制需采用多层控制环级联结构以实现最终控制目标。然而，各层环路之间耦合较强，参数整定过程复杂，不仅需要针对每一层单独调节，还高度依赖工程师的调参经验或相似系统的参数参考。因此，当设计新型四旋翼飞行器时，参数调节往往耗时较长，且难以保证所得参数为全局最优。

最优控制则提供了另一种思路：不再依赖人工经验逐一调试参数，而是通过定义代价函数，由数学优化方法直接求解使代价最小化的最优参数 $K$ 。

1.2 LQR 在控制界的地位

在控制算法的"知名度排行榜"上，PID 毫无争议排第一，而 LQR 大概能排到第二。它几乎出现在每一本现代控制理论的教材中，也是实际工程中应用最广泛的最优控制方法之一。

LQR 的全称是 Linear Quadratic Regulator（线性二次型调节器），这三个词各有含义：

词	含义
Linear（线性）	控制对象的动力学必须是线性的（或已被线性化）
Quadratic（二次型）	用来衡量"好坏"的代价函数是二次型函数
Regulator（调节器）	控制目标是将状态调节到零（即消除误差）

简单来说：LQR 是一种基于线性模型、通过最小化二次型代价函数来得到最优状态反馈增益的方法。

读完本文后，你将能够：

理解最优控制的基本思想：从"手动调参"到"定义代价函数，数学自动求解"
写出连续时间和离散时间的 LQR 代价函数，理解 $Q$ （状态权重）和 $R$ （控制权重）的物理含义
使用 Python（SciPy）求解代数 Riccati 方程，一步得到最优反馈增益矩阵 $K$
运用 Bryson 法则为四旋翼选择合适的 $Q$ 、 $R$ 初始值，并通过仿真迭代调优
理解 LQR 的稳定性保证和鲁棒性优势（无穷增益裕度 + 60° 相位裕度）
了解 LQR 的进阶扩展方向（LQI 积分控制、增益调度、LQG）

2. 线性二次型最优控制问题

2.1 问题的数学描述

考虑一个连续时间线性系统：

\begin{matrix} (1) & \dot{x} (t) = A (t) x (t) + B (t) u (t) \end{matrix}

其中 $x (t) \in R^{n}$ 是状态向量， $u (t) \in R^{m}$ 是控制输入向量。

我们定义一个性能指标（Performance Index，也叫代价函数 Cost Function）：

\begin{matrix} (2) & J = \frac{1}{2} \int_{t_{0}}^{t_{f}} [x (t)^{T} Q (t) x (t) + u (t)^{T} R (t) u (t)] d t + \frac{1}{2} x (t_{f})^{T} Q_{f} x (t_{f}) \end{matrix}

最优控制问题的提法是：找一个控制序列 $u^{*} (t)$ ，使得系统 (1) 在 $[t_{0}, t_{f}]$ 上稳定运行，同时代价函数 (2) 取到极小值。

2.2 代价函数每一项的含义

第一项： $x (t)^{T} Q x (t)$ ：状态误差的代价

$Q$ 是状态权重矩阵（ $n \times n$ ，半正定）
这一项衡量了状态偏离目标的程度。 $x$ 越大，代价越高
$Q$ 对角线上每个元素的大小，代表了对应状态的"重要性"
比如对于一个 3 维系统， $Q = [\begin{matrix} 1 & 0 & 0 \\ 0 & 10 & 0 \\ 0 & 0 & 100 \end{matrix}]$ 意味着第三个状态分量最重要：控制器会优先让它收敛

第二项： $u (t)^{T} R u (t)$ ：控制能量的代价

$R$ 是控制权重矩阵（ $m \times m$ ，正定）
这一项衡量了控制输入的大小。 $u$ 越大，代价越高
$R$ 必须是正定的：因为任何非零的控制输入都应该消耗能量、产生代价

第三项： $x (t_{f})^{T} Q_{f} x (t_{f})$ ：终端代价

强调在最终时刻 $t_{f}$ 状态也要尽量小
在无限时域问题中这一项会消失（后面会解释）

2.3 有限时域 LQR 的解

对于有限时域问题（ $t_{f}$ 是有限值），最优控制律的形式为：

\begin{matrix} (3) & u^{*} (t) = - R^{- 1} (t) B^{T} (t) P (t) x (t) \end{matrix}

其中 $P (t)$ 是 Riccati 矩阵微分方程的解，它是一个时变矩阵，这意味着反馈增益 $K (t) = - R^{- 1} B^{T} P (t)$ 也是时变的。

有限时域 LQR 的求解比较麻烦： $P (t)$ 需要从终端条件 $P (t_{f}) = Q_{f}$ 反向积分求解。好在我们通常不需要这样折腾，大多数工程问题都可以用更简单的无限时域 LQR 来解决。

3. 无限时域 LQR

3.1 从有限到无限

一个重要的发现是：当 $t_{f} \to \infty$ 时， $P (t)$ 会趋近于一个常值矩阵 $P$ 。这意味着反馈增益 $K$ 也变成了常值矩阵。

对于线性定常系统：

\begin{matrix} (4) & \dot{x} = A x + B u \end{matrix}

定义无限时域代价函数（终端项因 $x (\infty) \to 0$ 而消失）：

\begin{matrix} (5) & J = \int_{0}^{\infty} [x (t)^{T} Q x (t) + u (t)^{T} R u (t)] d t \end{matrix}

这就是我们常说的 Infinite Horizon LQR，也是工程实践中最常用的 LQR 形式。

3.2 代数 Riccati 方程

在无限时域下，原本的 Riccati 微分方程退化为代数 Riccati 方程（Algebraic Riccati Equation, ARE）：

\begin{matrix} (6) & P A + A^{T} P - P B R^{- 1} B^{T} P + Q = 0 \end{matrix}

解出 $P$ 之后，最优反馈增益矩阵为：

\begin{matrix} (7) & K = R^{- 1} B^{T} P \end{matrix}

控制律为：

\begin{matrix} (8) & u = - K x \end{matrix}

3.3 求解流程总结

对于使用者来说，LQR 的求解过程可以归纳为三步：

建立系统的状态空间模型，得到 $A$ 和 $B$ 矩阵
根据控制目标，选择合适的权重矩阵 $Q$ 和 $R$
求解 ARE 得到 $P$ ，然后计算 $K = R^{- 1} B^{T} P$

在工具层面：

MATLAB： $K = lqr (A, B, Q, R)$ 一步到位，直接返回增益矩阵 $K$ 。

Python（SciPy）：使用 scipy.linalg.solve_continuous_are 分两步完成：

python

from scipy.linalg import solve_continuous_are

P = solve_continuous_are(A, B, Q, R)   # 求解连续 ARE，得到 P 矩阵
K = np.linalg.inv(R) @ B.T @ P          # 由 P 计算反馈增益 K

solve_continuous_are 函数简介：

项目	说明
做了什么	求解连续时间代数 Riccati 方程 $A^{T} P + P A - P B R^{- 1} B^{T} P + Q = 0$
输入	$A$ （ $n \times n$ 状态矩阵）、 $B$ （ $n \times m$ 输入矩阵）、 $Q$ （ $n \times n$ 状态权重）、 $R$ （ $m \times m$ 控制权重）
输出	$P$ （ $n \times n$ 对称正定矩阵）：Riccati 方程的解
在工程中的角色	它是 LQR 设计流程的"最终计算步"：你负责提供模型 $(A, B)$ 和设计意图 $(Q, R)$ ，这个函数替你完成繁重的矩阵方程求解

函数内部使用的数值算法基于 QR/QZ 分解等成熟的线性代数技术，保证了求解的数值稳定性。调用者不需要理解这些细节，只需确认输入矩阵维度正确、 $(A, B)$ 能控、 $R$ 正定即可。

3.4 LQR 的稳定性保证

Infinite Horizon LQR 有一些重要的理论保证：

系统必须是能控的（ $(A, B)$ 可控），否则无法保证所有状态都能被驱动到零。
闭环系统是渐进稳定的，即 $A - B K$ 的所有特征值实部均为负。
如果 $(A, \sqrt{Q})$ 是可观测的，或者 $Q > 0$ ，则闭环稳定性有严格保证。

这些条件听起来可能较为抽象，但对于大多数实际的物理系统（包括四轴飞行器），它们都是自然满足的。

3.5 与 PID 的对比

LQR 和 PID 代表了两种截然不同的控制设计哲学：

	PID	LQR
结构	级联结构（外环+内环），需要为每个通道单独设计	全状态反馈，一个 $K$ 矩阵覆盖所有通道
调参方式	依赖工程经验逐层调参，通道间耦合相互影响	建立模型后，数学直接算出最优 $K$
参数个数	每层 3 个参数 × 多个通道(位置环3通道 + 姿态环3通道) → 参数数量快速增长	通过 Q 和 R 两个权重矩阵统一调整，参数有清晰的物理对应
耦合处理	手动处理，依赖工程师经验	自动处理所有交叉耦合
最优性	无	在给定 Q、R 下使代价函数取极小值
鲁棒性	依赖设计水平	有理论保证（无穷增益裕度 + 60° 相位裕度）

LQR 的优势在于：将"手动逐个调参"转变为"定义性能指标 → 数学自动求解"。工程师不再需要纠结于每个通道的 P/I/D 增益怎么设，而是回答一个更直观的问题："我更在意位置精度，还是更在意省电？"

4. LQR 的鲁棒性

LQR 除了"最优"之外，还有一个经常被忽略但非常重要的优点：出色的鲁棒性。

4.1 增益裕度：无穷大

考虑在控制输入的每个通道上乘以一个增益系数 $n_{i}$ （相当于输入通道的增益发生了变化），那么只要：

n_{i} \geq \frac{1}{2}

系统就依然稳定。换言之：

增益可以任意增大而不会失稳（无穷大增益裕度）
增益减小到原来的 50% 仍然稳定（至少 50% 的增益衰减裕度）

需要说明的是：上述结论针对的是每个输入通道独立的对角增益变化（ $u_{i} \to n_{i} u_{i}$ ， $n_{i} \geq 0.5$ ），而非任意形式的跨通道矩阵增益扰动。实际工程中如果各通道之间存在耦合的增益偏差（比如 $u \to N u$ ， $N$ 为非对角矩阵），这一结论并不直接适用。

4.2 相位裕度：至少 60°

LQR 在每个输入通道上具有至少 ±60° 的相位裕度。这意味着即使反馈信号存在时间延迟或相位滞后，LQR 控制器仍有很大的容忍空间。

4.3 为什么这很重要

鲁棒性意味着：你的模型不需要完美。即使实际的 $B$ 矩阵和你建模的有些出入（电机推力有偏差、力臂测量不准等），只要误差在一定范围内，LQR 控制器依然能保持稳定。这对于工程实践来说是一个巨大的优势。

5. LQR 的极限行为：Q 与 R 的博弈

理解 LQR 最关键的一点是认识 $Q$ 和 $R$ 的相对大小如何影响闭环行为。这一节将帮你建立直觉。

5.1 Control is Cheap： $Q ≫ R$

当 $Q$ 中的权重远大于 $R$ （或者说控制能量的代价"很便宜"，注意这里的"便宜"指的是控制能量代价低，而非控制器本身廉价），意味着：

你不在乎用了多少控制能量，你在意的只是状态误差要尽快消除
LQR 会给出高增益，响应极快，状态迅速收敛
闭环极点会趋向于 Butterworth 模式分布在左半平面，远离虚轴
如果系统有右半平面零点，它们会被驱赶到无穷远

代价：控制量可能非常大，容易触发执行器饱和，对噪声也更敏感。

5.2 Control is Expensive： $Q ≪ R$

当 $R$ 中的权重远大于 $Q$ （或者说控制"很昂贵"），意味着：

你不愿意付出太多控制能量，你宁可让状态收敛得慢一些
LQR 会给出低增益，响应平缓，控制量小
如果系统有开环不稳定极点，LQR 会以最小的控制量把它们搬到左半平面
如果系统开环稳定，增益会趋近于零，控制器几乎不干预

代价：响应很慢，状态误差较大。

5.3 实际中永远是折中

在真实的工程问题中，你永远在"控制很便宜"和"控制很昂贵"之间寻找平衡。没有一个放之四海皆准的 $Q$ 和 $R$ ，最优解取决于你的偏好。这也正是 LQR 设计的哲学：不存在普适的最优控制，只有在你指定了"什么重要"之后的最优控制。

6. 离散时间 LQR

前面所有讨论都是基于连续时间系统 $\dot{x} = A x + B u$ 。但实际控制器是在数字计算机上以固定频率运行的，需要离散化。

6.1 为什么需要离散化

你的控制器以固定周期 $Δ t$ 运行（比如 400Hz 对应 $Δ t = 0.0025 s$ ）：

读取传感器数据
运行控制算法
输出控制指令
等待下一个周期

因此需要将连续模型 $(A_{c}, B_{c})$ 转换为离散模型：

x_{k + 1} = A_{d} x_{k} + B_{d} u_{k}

6.2 精确离散化：矩阵指数法

一种高精度的离散化方法是利用矩阵指数。构建增广矩阵：

M = [\begin{matrix} A_{c} & B_{c} \\ 0 & 0 \end{matrix}]

计算：

e^{M Δ t} = [\begin{matrix} A_{d} & B_{d} \\ 0 & I \end{matrix}]

其中左上角 $n \times n$ 分块即为 $A_{d}$ ，右上角 $n \times m$ 分块即为 $B_{d}$ 。

这种方法比简单的欧拉近似 $A_{d} \approx I + A_{c} Δ t$ 精度高得多，是工程标准做法。

6.3 离散代数 Riccati 方程

得到 $(A_{d}, B_{d})$ 后，求解离散代数 Riccati 方程（DARE）：

A_{d}^{T} P A_{d} - P - A_{d}^{T} P B_{d} (B_{d}^{T} P B_{d} + R)^{- 1} B_{d}^{T} P A_{d} + Q = 0

离散反馈增益：

K = (B_{d}^{T} P B_{d} + R)^{- 1} B_{d}^{T} P A_{d}

离散控制律：

u_{k} = - K x_{k}

Python 中使用 scipy.linalg.solve_discrete_are 求解：

python

from scipy.linalg import solve_discrete_are

S = solve_discrete_are(Ad, Bd, Q, R)                # 求解离散 ARE，得到 P 矩阵
K = np.linalg.inv(Bd.T @ S @ Bd + R) @ (Bd.T @ S @ Ad)  # 由 P 计算离散反馈增益

solve_discrete_are 函数简介：

项目	说明
做了什么	求解离散时间代数 Riccati 方程 $A^{T} P A - P - A^{T} P B (B^{T} P B + R)^{- 1} B^{T} P A + Q = 0$
输入	$A_{d}$ （离散状态矩阵）、 $B_{d}$ （离散输入矩阵）、 $Q$ （状态权重）、 $R$ （控制权重）：与连续版本接口一致
输出	$P$ （ $n \times n$ 对称正定矩阵）：离散 Riccati 方程的解
与连续版本的区别	方程形式不同（差分代替微分），但输入输出接口和函数用途完全一致。离散版本直接适配数字控制器的步进逻辑 $x_{k + 1} = A_{d} x_{k} + B_{d} u_{k}$

这也是配套参考工程 lqr_controller.py 中实际使用的函数，工程中先对 $(A_{c}, B_{c})$ 做矩阵指数离散化得到 $(A_{d}, B_{d})$ ，然后调用 solve_discrete_are 求解离散反馈增益 $K$ ，最后在每个控制周期执行 $u_{k} = - K (x_{r e f} - x_{k})$ 。

7. 使用LQR算法来控制四旋翼飞行

有了前面的理论基础，现在我们来看一个具体的应用：用 LQR 实现四轴飞行器的定点悬停控制。

7.1 为什么 LQR 较为适合四轴飞行器

四轴飞行器是一个典型的多输入多输出（MIMO）系统：

4 个控制输入：四个电机的推力 $[T_{1}, T_{2}, T_{3}, T_{4}]$
12 个状态量：3 姿态角 + 3 角速度 + 3 位置 + 3 速度
强耦合：姿态和位置之间有动力学耦合（飞行器要平移必须先倾斜）

如果用 PID，你需要为每个通道单独设计控制器，还要想办法处理通道间的交叉耦合。而 LQR 天然适合这种场景：全状态反馈自动处理所有耦合关系，Q 和 R 矩阵提供了统一的调参入口。

7.2 建立状态空间模型

这是整个 LQR 设计中最关键的一步，主要是构建四旋翼物理模型的线性近似，这部分我们已经在之前的教程中详细讲解过，我们在这里再回顾一下。

状态向量（12 维）：

x = {[\begin{matrix} ϕ & θ & ψ & ω_{x} & ω_{y} & ω_{z} & p_{x} & p_{y} & p_{z} & v_{x} & v_{y} & v_{z} \end{matrix}]}^{T}

符号	含义	单位
$ϕ, θ, ψ$	滚转、俯仰、偏航角	rad
$ω_{x}, ω_{y}, ω_{z}$	三轴角速度	rad/s
$p_{x}, p_{y}, p_{z}$	世界坐标系位置	m
$v_{x}, v_{y}, v_{z}$	机体坐标系速度	m/s

关于坐标系不一致的说明：这里位置使用世界坐标系、速度使用机体坐标系，并非疏忽。
严格来说，位置的变化率 $\dot{p}$ 是世界坐标系下的速度 $v^{W}$ ，而状态中存储的是机体坐标系速度 $v^{B}$ ，两者通过旋转矩阵关联： $v^{W} = R_{W B} \cdot v^{B}$ （其中 $R_{W B}$ 是机体→世界的旋转矩阵，由欧拉角 $ϕ, θ, ψ$ 决定）。
在本工程的小角度假设下（ $ϕ, θ \approx 0$ ，且偏航角 $ψ$ 也被控制在 0 附近）， $R_{W B} \approx I_{3 \times 3}$ ，因此 $v^{W} \approx v^{B}$ ，两种速度近似相等。这样做的目的是简化线性模型的构建：省去了旋转矩阵在 A 矩阵中引入的复杂耦合项，使 LQR 设计更加简洁直接。
如果偏航角不接近 0，则需要先将机体速度旋转变换到世界坐标系后再参与位置计算，或者在偏航通道使用独立的坐标系处理。

控制向量（4 维）：

u = {[\begin{matrix} T_{1} & T_{2} & T_{3} & T_{4} \end{matrix}]}^{T} (单位: N)

系统矩阵 $A$ （在小角度假设 $ϕ, θ \approx 0$ 下）：

A = [\begin{matrix} 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & g & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ - g & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{matrix}]

$A$ 矩阵的核心结构可以用三句话概括：

姿态运动学（第 0-2 行）： $\dot{ϕ} = ω_{x}, \dot{θ} = ω_{y}, \dot{ψ} = ω_{z}$ ，角度变化率等于角速度
位置运动学（第 6-8 行）： ${\dot{p}}_{x} \approx v_{x}, {\dot{p}}_{y} \approx v_{y}, {\dot{p}}_{z} = v_{z}$ ，在世界坐标系下，位置的变化率应为世界坐标系速度 $v^{W}$ ，但在 $ψ \approx 0$ 的小角度假设下 $v^{W} \approx v^{B}$ （机体速度），因此这里直接用机体速度近似。Z 方向不受偏航角影响，严格成立
姿态-位置耦合（第 9-10 行）： ${\dot{v}}_{x} \approx g θ, {\dot{v}}_{y} \approx - g ϕ$ ，这是多旋翼飞行最本质的动力学关系：倾斜产生水平加速度。此处姿态角的正方向定义为： $θ > 0$ 时机头向下俯（前倾）， $ϕ > 0$ 时机身向右滚转（与 MuJoCo 坐标系约定一致）。在此约定下，前倾时重力沿机体 X 轴的正向分量 $g \sin θ \approx g θ$ 产生向前的水平加速度，右倾时重力沿机体 Y 轴的负向分量 $- g \sin ϕ \approx - g ϕ$ 产生向左的水平加速度。 $g = 9.81 {m/s}^{2}$ 在这里充当了"角度→加速度"的转换系数。

其余行（角速度行 3-5、Z 速度行 11）全为零，这些状态的变化完全由控制输入 $u$ 驱动，本身没有自发动力学。

输入矩阵 $B$ ：

B = [\begin{matrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \frac{d}{I_{x}} & - \frac{d}{I_{x}} & - \frac{d}{I_{x}} & \frac{d}{I_{x}} \\ - \frac{d}{I_{y}} & - \frac{d}{I_{y}} & \frac{d}{I_{y}} & \frac{d}{I_{y}} \\ - \frac{k}{I_{z}} & \frac{k}{I_{z}} & - \frac{k}{I_{z}} & \frac{k}{I_{z}} \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \frac{1}{m} & \frac{1}{m} & \frac{1}{m} & \frac{1}{m} \end{matrix}]

$B$ 矩阵各行对应电机推力如何产生状态变化：

行	影响	物理公式
3 ( ${\dot{ω}}_{x}$ )	Roll 角加速度	$\pm \frac{d}{I_{x}} \cdot T_{i}$
4 ( ${\dot{ω}}_{y}$ )	Pitch 角加速度	$\pm \frac{d}{I_{y}} \cdot T_{i}$
5 ( ${\dot{ω}}_{z}$ )	Yaw 角加速度	$\pm \frac{k}{I_{z}} \cdot T_{i}$
11 ( ${\dot{v}}_{z}$ )	Z 轴加速度	$\frac{1}{m} \sum T_{i}$

正负号由电机的安装位置和旋转方向决定（X 型四轴构型）。

7.3 Q 和 R 的选择：Bryson 法则

对于初学者来说，选择 Q 和 R 可能感觉很"玄学"。Bryson 法则提供了一个很好的起点：

Q_{i i} = \frac{1}{(状态 i 的最大可接受误差)^{2}}, R_{j j} = \frac{1}{(控制输入 j 的最大可接受变化量)^{2}}

例如，如果你认为位置误差不应超过 1m，则 $Q_{p o s} = 1 / 1^{2} = 1.0$ ，如果希望电机推力增量不超过 0.1N，则 $R = 1 / {0.1}^{2} = 100.0$ 。

参考工程中的实际 $Q$ 设置（12 维对角阵）：

滚转/俯仰角权重：5.0（姿态纠正比较积极）
偏航角权重：10.0（偏航纠正最积极）
角速度权重：0.1（允许角速度有一定波动，相当于增加阻尼）
位置权重：1.0（中等精度要求）
速度权重：0.1（阻尼项，防止速度超调）
控制权重 $R$ ：100.0（推力变化偏保守，优先保证平滑）

7.4 实际工程中的额外设计

理论上的 LQR 只需 $u = - K x$ 就完事了。但在实际工程中，还有几件事需要考虑。

悬停前馈：LQR 计算的是增量控制 $Δ u$ 。四轴飞行器需要先有一个基础推力来抵消重力：

T_{h o v e r} = \frac{m g}{n} = \frac{0.2125 \times 9.81}{4} \approx 0.521 N

最终输出为 $u_{f i n a l} = Δ u + T_{h o v e r}$ 。

控制限幅：LQR 不关心电机的物理极限，所以需要手动限幅：

| Δ u_{i} | \leq min (T_{m a x} - T_{h o v e r}, T_{h o v e r} - T_{m i n})

预测补偿（工程技巧，非 LQR 理论本身）：电机有响应延迟（一阶惯性，时间常数 $τ \approx 0.03 s$ ）。如果直接用当前状态算误差再反馈，会有相位滞后。一个工程上的补偿方法是用离散模型 $x_{k + 1} = A_{d} x_{k} + B_{d} u_{k}$ 向前递推几步，用预测状态代替当前状态来计算控制误差。这相当于让控制器"看到"状态接下来的走向，有效避免了延迟导致的振荡。

注意：这一步并非 LQR 的标准组成部分。如果采样率足够高（控制周期远小于执行器时间常数），或者执行器响应足够快，这一步可以省略，直接用当前状态反馈即可。盲目照搬但预测步数与实际延迟不匹配，反而可能引入额外的模型误差。

连续模型离散化：控制器以固定频率运行（参考工程中为 400Hz， $Δ t = 0.0025 s$ ），需用矩阵指数法将 $(A_{c}, B_{c})$ 离散化为 $(A_{d}, B_{d})$ ，再求解离散 ARE 得到 $K$ 。

8. LQR 的调参实战

8.1 增量调参策略

用 Bryson 法则确定 Q 和 R 的初始值
运行仿真，观察各状态的时域响应
某个状态收敛太慢 → 增大对应的 Q 值
控制量振荡或频繁饱和 → 增大 R 值
某状态超调严重 → 增大其对应的导数项权重（角速度/速度）来增加阻尼
重复 2-5 直到满意

8.2 常见问题速查

现象	可能原因	调整方向
响应太慢	Q 太小或 R 太大	增大 Q / 减小 R
超调振荡	阻尼不足	增大角速度/速度权重
控制量饱和	R 太小	增大 R
稳态误差	有未建模扰动	考虑加积分项（LQI）
某通道耦合异常	线性化不准	检查模型参数

9. LQR 的扩展：输出调节、跟踪与积分控制

基本的 LQR（Infinite Horizon Regulator）做的是"把状态调节到零"。但在实际应用中，我们往往需要更多功能。

9.1 输出调节（Output LQR）

如果我们关心的不是全部状态，而是某些输出 $y = C x$ ，可以把代价函数改成用输出表示：

J = \int_{0}^{\infty} [y^{T} Q_{y} y + u^{T} R u] d t = \int_{0}^{\infty} [x^{T} (C^{T} Q_{y} C) x + u^{T} R u] d t

只需将原来的 $Q$ 替换为 $C^{T} Q_{y} C$ （同样是半正定的），求解过程完全一样。

9.2 从定点调节到轨迹跟踪

LQR 的默认假设是将状态调节到零。但在实际应用中，我们通常需要跟踪非零的目标。根据目标的特性，有三种不同的处理方式：

（1）定点调节（Setpoint Tracking）

如果目标状态 $x_{d}$ 是固定的（例如悬停在某个位置），处理方式最简单：令误差 $e = x - x_{d}$ ，在目标点附近线性化：

\dot{e} \approx A e + B Δ u

这里 $A$ 和 $B$ 是在 $x_{d}$ 处求得的常值矩阵。求解一次 ARE 即可得到一个固定的反馈增益 $K$ ，控制律为 $u = u_{d} - K e$ 。本文配套的四轴悬停控制就属于这种情况：目标位置固定，一个常值 $K$ 即可。

（2）严格时变轨迹跟踪（Finite-Horizon LQR）

如果目标 $x_{d} (t)$ 是一条随时间变化的轨迹（例如跟踪一个圆形航线），严格的最优做法是求解有限时域 LQR 问题：使用有限时域代价函数 $J = \int_{0}^{t_{f}} (\dots) d t$ ，求解 Riccati 微分方程，得到一个时变增益 $K (t)$ 。这需要已知完整的未来参考轨迹，计算量也更大。

（3）增益调度（Gain Scheduling）：工程近似

对于大范围机动的非线性系统，一种工程简化做法是：在状态空间的多个工作点分别线性化并计算对应的固定增益 $K_{i}$ ，运行时根据当前状态查找或插值得到 $K$ 。这种方法不是最优的，但在实践中往往足够好用，尤其适用于工作点变化缓慢的场景。

总结：定点跟踪 → 常值 $K$ ，时变轨迹严格解 → 有限时域 LQR 得 $K (t)$ ，大范围非线性机动 → 增益调度（工程近似）。三种方式的适用场景和理论严格性各不相同，不可混为一谈。

9.3 在 LQR 中引入积分控制（LQI）

标准 LQR 没有积分作用，可能产生稳态误差。解决方法是将积分器状态扩充到系统状态中：

定义积分误差 $z = \int e d t$ ，构建增广系统：

[\begin{matrix} \dot{x} \\ \dot{z} \end{matrix}] = [\begin{matrix} A & 0 \\ - C & 0 \end{matrix}] [\begin{matrix} x \\ z \end{matrix}] + [\begin{matrix} B \\ 0 \end{matrix}] u

然后对这个增广系统设计 LQR，得到的 $K$ 自然包含了积分作用。

这个思路说明：LQR 与积分控制、MPC 等方法可以自然地融合。LQR 提供的不是一个孤立的控制器，而是一个可以灵活扩展的控制设计框架。

10. 总结与进阶方向

10.1 核心要点

LQR 是一种最优状态反馈控制：不指定极点，而是通过最小化代价函数来求 $K$
代价函数 $J = \int (x^{T} Q x + u^{T} R u) d t$ ： $Q$ 控制性能追求， $R$ 控制能量代价
求解核心是代数 Riccati 方程：MATLAB 的 lqr() 或 Python 的 solve_continuous_are() 一步搞定
LQR 有极好的鲁棒性：无穷增益裕度 + 60° 相位裕度
离散化是工程必需：用矩阵指数法精确离散化，再用离散 ARE 求解
调参从 Bryson 法则出发：跑仿真，观察，迭代调整

10.2 LQR 的局限

只适用于线性系统（非线性系统需要先线性化）
需要全状态可测或可估计（实际中常配合 Kalman 滤波器）
模型精度直接影响控制效果
标准形式没有积分作用（可通过 LQI 扩展）

本章参考资料与引申阅读已汇总至参考资料。

4.1 LQR ​

0. 前言 ​

1.1 从"能控"到"最优" ​

1.2 LQR 在控制界的地位 ​

目录 ​

2. 线性二次型最优控制问题 ​

2.1 问题的数学描述 ​

2.2 代价函数每一项的含义 ​

2.3 有限时域 LQR 的解 ​

3. 无限时域 LQR ​

3.1 从有限到无限 ​

3.2 代数 Riccati 方程 ​

3.3 求解流程总结 ​

3.4 LQR 的稳定性保证 ​

3.5 与 PID 的对比 ​

4. LQR 的鲁棒性 ​

4.1 增益裕度：无穷大 ​

4.2 相位裕度：至少 60° ​

4.3 为什么这很重要 ​

5. LQR 的极限行为：Q 与 R 的博弈 ​

5.1 Control is Cheap：Q≫R ​

5.2 Control is Expensive：Q≪R ​

5.3 实际中永远是折中 ​

6. 离散时间 LQR ​

6.1 为什么需要离散化 ​

6.2 精确离散化：矩阵指数法 ​

6.3 离散代数 Riccati 方程 ​

7. 使用LQR算法来控制四旋翼飞行 ​

7.1 为什么 LQR 较为适合四轴飞行器 ​

7.2 建立状态空间模型 ​

7.3 Q 和 R 的选择：Bryson 法则 ​

7.4 实际工程中的额外设计 ​

8. LQR 的调参实战 ​

8.1 增量调参策略 ​

8.2 常见问题速查 ​

9. LQR 的扩展：输出调节、跟踪与积分控制 ​

9.1 输出调节（Output LQR） ​

9.2 从定点调节到轨迹跟踪 ​

9.3 在 LQR 中引入积分控制（LQI） ​

10. 总结与进阶方向 ​

10.1 核心要点 ​

10.2 LQR 的局限 ​